from pb.ros.geometry_msgs import Pose_pb2
from pb.ros.geometry_msgs import PoseStamped_pb2
from pb.ros.nav_msgs import Odometry_pb2
from pb.ros.visualization_msgs import Marker_pb2
from pb.ros.geometry_msgs import Point_pb2
import proxy

import os
import time
import math
import threading
import sys

node = proxy.Connector('tcp://172.22.56.12:15555') #在709的电脑上使用该IP
#node = proxy.Connector('tcp://12.12.12.204:15555') #在集群的电脑上使用该IP
publish_cmd_goal = node.Advertise('FK','M300_0', '/FK/M300_0/cmd_goal', PoseStamped_pb2.PoseStamped) #设置终点（相对位置）

                
class Publish:
    #设置终点（相对位置）
    def cmd_goal(self,x1, y1, z1, x, y, z, w):
        cmd_goal = PoseStamped_pb2.PoseStamped()
        cmd_goal.pose.position.x = x1
        cmd_goal.pose.position.y = y1
        cmd_goal.pose.position.z = z1
        cmd_goal.pose.orientation.x = x
        cmd_goal.pose.orientation.y = y
        cmd_goal.pose.orientation.z = z
        cmd_goal.pose.orientation.w = w
        cmd_goal.header.frame_id = "world"
        publish_cmd_goal.Publish(cmd_goal)

    def pub(self):
        x1 = 200
        y1 = 0
        z1 = 3
        x = 0
        y = 0
        z = 0
        w = 1
        self.cmd_goal(x1, y1, z1, x, y, z, w)

def main(args):
    publish = Publish()
    thread_pub = threading.Thread(target = publish.pub)
    thread_pub.start()
    
    n = input('')


if __name__ == '__main__':
    main(sys.argv)
